#include "Line.h"

Line::Line(int id, double orientation, double length, int end1, int end2, int mid, double *depths){
    this->id = id;
    this->orientation = orientation;
    this->length = length;
    linePoints = new int[3];
    linePoints[0] = end1;
    linePoints[1] = end2;
    linePoints[2] = mid;
    this->depths = depths;
}

int Line::getId(){
    return id;
}

double Line::getOrientation(){
    return orientation;
}

double Line::getLength(){
    return length;
}

double *Line::getDepths(){
    return depths;
}

int *Line::getPoints(){
    return linePoints;
}

//-----------------LineGroup------------------//

/**
*  Returns true if two lines have endpoints within ADJ_THRESH of each other.
*/
bool checkAdj(int *line1_points, int *line2_points){
    if(pointDist(line1_points[0],line2_points[0]) <= ADJ_THRESH || pointDist(line1_points[0],line2_points[1]) <= ADJ_THRESH ||
       pointDist(line1_points[1],line2_points[0]) <= ADJ_THRESH || pointDist(line1_points[1],line2_points[1]) <= ADJ_THRESH)
        return 1;
    else
        return 0;
}

/**
*  Returns true if two lines have orientations w/in PERP_THRESH of being
*  PI/2 apart. Only called on adjacent lines.
*/
bool checkPerp(double line1_orient, double line2_orient){  
    double d_orient = abs(line1_orient - line2_orient);
    if(d_orient>(PI/2)){
        line1_orient += ((line1_orient < (PI/2)) ? PI : -1*PI);
        d_orient = abs(line1_orient - line2_orient);
    }
    if(abs(d_orient - PI/2) <= PERP_THRESH)
        return 1;
    else 
        return 0;
}

/**
*  Returns true if two lines have orientations w/in PAR_THRESH of each other,
*  providing they are also somewhat overlapping horizontally or vertically
*  (that is to say, they have an 'above' or 'leftOf' relationship).
*/
bool checkPar(int posInfo, double line1_orient, double line2_orient){ 
    if(posInfo!=0){
        double d_orient = abs(line1_orient - line2_orient);
        if(d_orient>(PI/2)){
            line1_orient += ((line1_orient < (PI/2)) ? PI : -1*PI);
            d_orient = abs(line1_orient - line2_orient);
        }
        if(d_orient <= PAR_THRESH)
            return 1;
    }
    return 0;
}

/**
*  Returns a number corresponding to the positional relationship between the lines. For example, line 1
*  is 'above' line2 iff they have some horizontal overlap in their x coordinates and the centerpoint of
*  line1 is higher (<) that of line2. Leftof operates the same way, if you swap the x and y. These can 
*  happen concurrently (eg. above-left). The numbers are defined as follows -
*    0 -> no positional relationship
*    1 -> l1 leftOf l2
*    2 -> l2 leftOf l1
*    3 -> l1 above l2
*    4 -> l1 above l2; l1 leftOf l2
*    5 -> l1 above l2; l2 leftOf l1
*    6 -> l2 above l1
*    7 -> l2 above l1; l1 leftOf l2
*    8 -> l2 above l1; l2 leftOf l1
*/
int getPosInfo(int *line1_points, int *line2_points){ 
    int pos = 0;
    int *line1_start = addrToCoords(line1_points[0]);
    int *line1_end = addrToCoords(line1_points[1]);
    int *line1_mid = addrToCoords(line1_points[2]);
    int *line2_start = addrToCoords(line2_points[0]);
    int *line2_end = addrToCoords(line2_points[1]);
    int *line2_mid = addrToCoords(line2_points[2]);
    int line1_x_min = ((line1_start[0]<=line1_end[0]) ? line1_start[0] : line1_end[0]);
    int line1_x_max = ((line1_start[0]>line1_end[0]) ? line1_start[0] : line1_end[0]);
    int line1_y_min = ((line1_start[1]<=line1_end[1]) ? line1_start[1] : line1_end[1]);
    int line1_y_max = ((line1_start[1]>line1_end[1]) ? line1_start[1] : line1_end[1]);
    int line2_x_min = ((line2_start[0]<=line2_end[0]) ? line2_start[0] : line2_end[0]);
    int line2_x_max = ((line2_start[0]>line2_end[0]) ? line2_start[0] : line2_end[0]);
    int line2_y_min = ((line2_start[1]<=line2_end[1]) ? line2_start[1] : line2_end[1]);
    int line2_y_max = ((line2_start[1]>line2_end[1]) ? line2_start[1] : line2_end[1]);
    if((line2_start[1] >= line1_y_min && line2_start[1] <= line1_y_max) || (line2_end[1] >= line1_y_min && line2_end[1] <= line1_y_max) ||
       (line2_mid[1] >= line1_y_min && line2_mid[1] <= line1_y_max) || (line1_mid[1] >= line2_y_min && line1_mid[1] <= line2_y_max)){
        if(line1_mid[0] < line2_mid[0]) pos += 1;   //l1 left l2
        else pos += 2;                            //l1 right l2
    }
    if((line2_start[0] >= line1_x_min && line2_start[0] <= line1_x_max) || (line2_end[0] >= line1_x_min && line2_end[0] <= line1_x_max) ||
       (line2_mid[0] >= line1_x_min && line2_mid[0] <= line1_x_max) || (line1_mid[0] >= line2_x_min && line1_mid[0] <= line2_x_max)){
        if(line1_mid[1] < line2_mid[1]) pos += 3;   //l1 above l2
        else pos += 6;                            //l1 below l2
    }
    return pos;
}

/**
*  Takes an array of LSRs, converts each to a line and stores it in an array of Lines.
*/
void linesFromLSRs(Line *Lines, LSR *LSRs, bool isGoal){
    int n_lines = (isGoal ? N_GOAL_LINES : N_CURR_LINES);
    for(int i=0; i<n_lines; i++){
        LSR LSR_i = LSRs[i];
        int *ends = LSR_i.getLinePoints();
        int mid = LSR_i.getCentroid();
        double *depths = LSR_i.getDepths();
        Line l_i (i, LSR_i.getOrientation(), LSR_i.getLength(), ends[0], ends[1], mid, depths);
        Lines[i] = l_i;
    }
}

/**
*  A constructor for a LineGroup given an array of lines. This is very long due to setting 
*  up the relationship matrices.
*/
LineGroup::LineGroup(Line *Lines, bool isGoal){
    n_lines = (isGoal ? N_GOAL_LINES : N_CURR_LINES); 
    this->Lines = Lines;

    int x_sum = 0;
    int y_sum = 0;
    double orient_sum = 0.0;
    double length_sum = 0.0;
    for(int i=0; i<n_lines; i++){
        Line l_i = Lines[i];
        int *l_i_points = l_i.getPoints();
        int *l_i_mid = addrToCoords(l_i_points[2]);
        x_sum += l_i_mid[0];
        y_sum += l_i_mid[1];
        orient_sum += l_i.getOrientation();
        length_sum += l_i.getLength();
    }

    if(isGoal){
        G_MEANS[0] = coordsToAddr((x_sum/n_lines), (y_sum/n_lines));
        G_MEANS[1] = orient_sum/n_lines;
        G_MEANS[2] = length_sum/n_lines;
    } else {
        C_MEANS[0] = coordsToAddr((x_sum/n_lines), (y_sum/n_lines));
        C_MEANS[1] = orient_sum/n_lines;
        C_MEANS[2] = length_sum/n_lines;
    }

    R_adj = new bool[n_lines*n_lines];
    R_perp = new bool[n_lines*n_lines];
    R_par = new bool[n_lines*n_lines];
    R_above = new bool[n_lines*n_lines];
    R_left = new bool[n_lines*n_lines];
    for(int i=0; i<n_lines; i++){
        int addr = relAddr(i,i);
        R_adj[addr] = 0;
        R_perp[addr] = 0;
        R_par[addr] = 0;
        R_above[addr] = 0;
        R_left[addr] = 0;
    }
    for(int i=0; i<n_lines-1; i++){
        Line l_i = Lines[i];        
        int *l_i_points = l_i.getPoints();
        double l_i_orientation = l_i.getOrientation();
        for(int j=i+1; j<n_lines; j++){
            Line l_j = Lines[j];
            int *l_j_points = l_j.getPoints();
            double l_j_orientation = l_j.getOrientation();
            int ij = relAddr(i,j);
            int ji = relAddr(j,i);
            R_adj[ij] = checkAdj(l_i_points, l_j_points);   
            R_adj[ji] = 0;
            if(R_adj[ij]){
	        R_perp[ij] = checkPerp(l_i_orientation, l_j_orientation); 
	        R_perp[ji] = 0;
            } else {                //don't want perpendicular lines that aren't adjacent
                R_perp[ij] = 0;
                R_perp[ji] = 0;
            }
            int position = getPosInfo(l_i_points, l_j_points);
            R_par[ij] = checkPar(position, l_i_orientation, l_j_orientation);   
	    R_par[ji] = 0; 
            switch(position){
                case 0:    // no positional relations
                    R_above[ij] = 0;
                    R_above[ji] = 0;
                    R_left[ij] = 0;
                    R_left[ji] = 0;
                    break;
                case 1:    // l1 left l2
                    //cout << i << " left of " << j << endl;
                    R_above[ij] = 0;
                    R_above[ji] = 0;
                    R_left[ij] = 1;
                    R_left[ji] = 0;
                    break;
                case 2:    // l1 right l2
                    //cout << i << " right of " << j << endl;
                    R_above[ij] = 0;
                    R_above[ji] = 0;
                    R_left[ij] = 0;
                    R_left[ji] = 1;
                    break;
                case 3:    // l1 above l2
                    //cout << i << " above " << j << endl;
                    R_above[ij] = 1;
                    R_above[ji] = 0;
                    R_left[ij] = 0;
                    R_left[ji] = 0;
                    break;
                case 4:    // l1 above-left l2
                    //cout << i << " above and left of " << j << endl;
                    R_above[ij] = 1;
                    R_above[ji] = 0;
                    R_left[ij] = 1;
                    R_left[ji] = 0;
                    break;
                case 5:    // l1 above-right l2
                    //cout << i << " above and right of " << j << endl;
                    R_above[ij] = 1;
                    R_above[ji] = 0;
                    R_left[ij] = 0;
                    R_left[ji] = 1;
                    break;
                case 6:    // l1 below l2
                    //cout << i << " below " << j << endl;
                    R_above[ij] = 0;
                    R_above[ji] = 1;
                    R_left[ij] = 0;
                    R_left[ji] = 0;
                    break;
                case 7:    // l1 below-left l2
                    //cout << i << " below and left of " << j << endl;
                    R_above[ij] = 0;
                    R_above[ji] = 1;
                    R_left[ij] = 1;
                    R_left[ji] = 0;
                    break;
                case 8:    // l1 below-right l2
                    //cout << i << " below and right of " << j << endl;
                    R_above[ij] = 0;
                    R_above[ji] = 1;
                    R_left[ij] = 0;
                    R_left[ji] = 1;
                    break;
                default:
                    cout << "What?" << endl;
            }
                
        }
    }
}

int LineGroup::getN_lines(){
    return n_lines;
}

Line *LineGroup::getLines(){
    return Lines;
}

Line LineGroup::getLineAt(int index){
    return Lines[index];
}

bool *LineGroup::getR_adj(){
    return R_adj;
}

bool *LineGroup::getR_perp(){ 
    return R_perp;
}

bool *LineGroup::getR_par(){
    return R_par;
}

bool *LineGroup::getR_above(){
    return R_above;
}

bool *LineGroup::getR_left(){
    return R_left;
}

/*double *LineGroup::getA_means(){
    return a_means;
}*/

/**
*  Conversion from line 'coordinates' to a spot in the relationship matrices.
*/
int LineGroup::relAddr(int line1, int line2){
    int retAddr = line1*n_lines + line2;
    return retAddr;
}

/**
*  Conversion from an index in the relationship matrices to the lines represented by that index.
*/
int *LineGroup::relLines(int addr){
    int *ret= new int[2]; //return {x,y}
    ret[0] = addr%n_lines;
    ret[1] = addr/n_lines;
    return ret;
}

/**
*  Given L, which is a LineGroup, draws all of the lines from L onto the image provided.
*/
void drawL(LineGroup target, char *log_path, Mat cleanImg, bool isGoal){
    char drawn_image_name[100];
    char clean_image_name[100];
    Mat imgDraw = cleanImg.clone(); 
    if(isGoal){
        sprintf(drawn_image_name,"%sgoalLines.jpg", log_path);
        sprintf(clean_image_name,"%sgoalImg.jpg", log_path);
    } else {
        sprintf(drawn_image_name,"%scurrLines.jpg", log_path);  
        sprintf(clean_image_name,"%scurrImg.jpg", log_path);    
    }   
    int n_lines = target.getN_lines();
    for(int i=0; i<n_lines; i++){
	Line l_i = target.getLineAt(i);
	int *points = l_i.getPoints();
	int *start = addrToCoords(points[0]);
	int *end = addrToCoords(points[1]);
	Point pt1 = Point(start[0],start[1]);
	Point pt2 = Point(end[0],end[1]);
	line(imgDraw, pt1, pt2, Scalar(0,0,255), 3);
    }
    imwrite(drawn_image_name, imgDraw);
    imwrite(clean_image_name, cleanImg);
}

